﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using UnityEngine;

namespace PathFinding
{
	public class PFNodeHeap
	{
		private PFNode[] items;
		private int tail = 0;
		private int capacity;

		public int Count
		{
			get
			{
				return tail;
			}
		}

		public PFNodeHeap(int capacity = 4)
		{
			this.capacity = capacity;
			items = new PFNode[capacity];
		}

		private void Grow()
		{
			int newCapacity = capacity * 2;
			PFNode[] newItems = new PFNode[newCapacity];
			Array.Copy(items, newItems, capacity);
			items = newItems;
			capacity = newCapacity;
		}

		public void Add(PFNode item)
		{
			if (Count == capacity) Grow();

			item.lastHeapIndex = tail;
			items[tail++] = item;
			SortUp(item);
		}

		public PFNode Pop()
		{
			if (Count == 0) throw new InvalidOperationException("PFNodeHeap is empty");

			PFNode firstItem = items[0];
			items[0] = items[--tail];
			items[0].lastHeapIndex = 0;
			SortDown(items[0]);
			return firstItem;
		}

		public void UpdateItem(PFNode item)
		{
			SortUp(item);
		}

		public bool Contains(PFNode item)
		{
			if (item.lastHeapIndex >= tail) return false;
			return items[item.lastHeapIndex] == item;
		}

		void SortDown(PFNode item)
		{
			while (true)
			{
				int childIndexLeft = item.lastHeapIndex * 2 + 1;
				int childIndexRight = item.lastHeapIndex * 2 + 2;
				int swapIndex = 0;

				if (childIndexLeft < tail)
				{
					swapIndex = childIndexLeft;

					if (childIndexRight < tail)
					{
						if (items[childIndexLeft].CompareTo(items[childIndexRight]) < 0)
						{
							swapIndex = childIndexRight;
						}
					}

					if (item.CompareTo(items[swapIndex]) < 0)
					{
						Swap(item, items[swapIndex]);
					}
					else
					{
						return;
					}

				}
				else
				{
					return;
				}

			}
		}

		private void SortUp(PFNode item)
		{
			int parentIndex = (item.lastHeapIndex - 1) / 2;

			while (true)
			{
				PFNode parentItem = items[parentIndex];
				if (item.CompareTo(parentItem) > 0)
				{
					Swap(item, parentItem);
				}
				else
				{
					break;
				}

				parentIndex = (item.lastHeapIndex - 1) / 2;
			}
		}

		private void Swap(PFNode a, PFNode b)
		{
			items[a.lastHeapIndex] = b;
			items[b.lastHeapIndex] = a;
			int itemAIndex = a.lastHeapIndex;
			a.lastHeapIndex = b.lastHeapIndex;
			b.lastHeapIndex = itemAIndex;
		}
	}



	public class PFNode : IComparable<PFNode>
	{
		/// <summary>
		/// g 是“迄今为止”从起点到当前节点的最短路径，是一种路径花销估计值
		/// h 是顶点到目标的距离的估计
		/// </summary>
		public int gCost, hCost;
		public PFNode parent;
		public Vector2Int pos;
		public PFPoint point;
		public int lastHeapIndex;

		public PFNode(Vector2Int pos, PFPoint point)
		{
			this.pos = pos;
			this.point = point;
		}

		public PFNode(Vector2Int pos, uint cost)
		{
			this.pos = pos;
			gCost = (int)cost;
			hCost = 0;
		}

		public int FCost => gCost + hCost;

		public int CompareTo(PFNode other)
		{
			if (other == null) return -1;
			int result = FCost.CompareTo(other.FCost);
			if (result == 0) result = gCost.CompareTo(other.gCost);
			return -result;
		}



	}
}